#ifndef MAPPING_NAVIGATION_PLANNER
#define MAPPING_NAVIGATION_PLANNER

#include "mapping/map_loader.h"
#include "mapping/clear_destinations.h"
#include "mapping/show_path.h"
#include "mapping/plan_with_gps.h"

#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Path.h>

#include <GeographicLib/LocalCartesian.hpp>

#include <ros/ros.h>
#include <glog/logging.h>
#include <mutex>
#include <Eigen/Dense>

class Navigation_planner{
public:
    struct LLA{
        double latitude  = 0.;
        double longitude = 0.;
        double altitude  = 0.;
    };
    Navigation_planner(ros::NodeHandle& nh_);
private:
    void DEBUG_pub_grid_map();
    void DEBUG_print_destinations_lla();

    void clicked_point_callback(const geometry_msgs::PointStampedConstPtr& msg_in);
    bool service_clear_destinations_callback(mapping::clear_destinationsRequest& req,mapping::clear_destinationsResponse& res);
    bool server_show_path_callback(mapping::show_pathRequest& req,mapping::show_pathResponse& res);
    bool server_plan_with_gps_callback(mapping::plan_with_gpsRequest& req,mapping::plan_with_gpsResponse& res);
    bool transform_destinations_to_LLA();
    void save_lla_to_txt();
private:

    std::mutex mtx;
    std::shared_ptr<Map_loader> map_loader;
    
    bool plan_with_gps;//如果设置成true , 会从yaml中读取原点的GPS坐标(要求目前栅格地图也是ENU系的)，将destinations中的所有点都转换成LLA (map -> GPS)
    GeographicLib::LocalCartesian geo_converter;
    bool geo_converter_inited;

    ros::NodeHandle nh;
    ros::Publisher pub_grid_map;
    ros::Publisher pub_path;             // only for visualization
    ros::Subscriber sub_clicked_point;
    ros::ServiceServer server_clear_destinations; //将destinations 中的点清空
    ros::ServiceServer server_show_path; // only for visualization 将destinations 中的点连成一条path 并发布显示
    ros::ServiceServer server_plan_with_gps; // only work with plan_with_gps 将destinations 的点转换成gps坐标

    nav_msgs::OccupancyGrid grid_mapROS;
    
    unsigned char* grid_map;
    std::string map_yaml_path,map_data_path;
    Map_info map_info;

    std::vector<Navigation_planner::LLA> destinations_lla;
    Navigation_planner::LLA origin_gps;
    std::vector<Eigen::Vector3d> destinations;
};

#endif